/****************************************************************************\
 * @file     BR_ServoMotor.h
 * @author   傅帅
 * @date     2019/12/30
 * @brief    舵向电机
 *
 ******************************************************************************
 * @note    C6120板通讯协议： 
            维护最多16个C610/620的驱动，编号0-7 8-15分别在CAN1 CAN2
            可接受两路串口消息,处理逻辑相同,从哪个串口初始化的返回也发回那个串口
            PID的目标值为相对于逻辑零位的总counts

            通讯协议如下:
            1.驱动上发:四个舵向电机的绝对计数(以逻辑0位为0顺正逆负的count)
                       'A'+0x00+电机编号+int32_t count+校验
            2.初始化帧应答:收到初始化帧后回发消息，保证初始化成功
                       'A'+0x01+校验
            3.归零帧应答:收到归零帧后回发消息，保证归零成功
                       'A'+0x03+校验
            主控下发:
            1.初始化帧:对单个电机进行使能和速度环位置环PID参数设置
                       'A'+0x01+电机编号(char 0-15)
                        +设置零点uint16_t counts+最小安全计数uint32_t counts+最大安全计数uint32_t counts
                        +p+i+d+i_lim+total_lim(vel)
                        +p+i+d+i_lim+total_lim(pos)+校验

            2.控制帧:  对单个电机进行速度/位置/电流控制
                       'A'+0x02+电机编号+'v'/'p'/'c'+int32_t 目标值+校验

            3.归零帧:  告知该电机已回到逻辑零位附近,收到该帧则将累计圈数清零
                       'A'+0x03+电机编号+校验           
 ****************************************************************************/
#ifndef BR_SERVO_MOTOR_H
#define BR_SERVO_MOTOR_H
#include "FreeRTOS.h"
#include "task.h"
#include "timers.h"
#include "BR_Error.h"

/* FreeRTOS配置 */
#define BR_ServoMotorHandlerTaskPrio 14
#define BR_ServoMotorHandlerTaskStkSize 512
/* 其他配置 */
#define SERVO_MOTOR_BAUDRATE    1382400 // 波特率
#define SERVO_MOTOR_TX_MAX_LEN  100     // 发送最大字节数
#define SERVO_MOTOR_TX_PRIO     14      // 串口发送DMA中断优先级
#define SERVO_MOTOR_RX_MAX_LEN  50      // 接收最大字节数
#define SERVO_MOTOR_RX_PRIO     14      // 串口接受串口中断优先级

#define GO_LOGIC_ZERO_WINDOW    0.002f     // 归零逻辑允许误差窗口[rad] 0.002 -> 0.1°
#define GO_LOGICZERO_WINDOW_COUNTS GO_LOGIC_ZERO_WINDOW / PI * 4096 * BR_configServoReductionRatio  // 窗口计数

/* 舵向电机结构体 */
typedef struct
{
    float RelativePos[4];
    void ( *fServoMotorsPosSet )(float *pos_values); // 舵向电机控制，输入为长度为四的float数组
}BR_ServoMotor_t;

void BR_vServoMotorHandlerTask(void);
void BR_vServoMotorInit(void);
void BR_vServoMotorInitSend(float VelP, float VelI, float VelD, float CurrentI_Limit, float Current_Limit,
                            float PosP, float PosI, float PosD, float VelI_Limit,float Vel_Limit);
void BR_vServoMotorsPosSet(float *Poses);
void BR_vServoMotorsVelSet(int16_t *Vels);
void BR_vServoMotorsCurSet(int16_t *Curs);
void BR_vServoMotorsGoLogicZeroPos(void);
void BR_vServoMotorsGoLogicZeroRobotThetaPos(void);
void BR_vServoMotorCalibration(uint16_t CalVel);
void BR_vCLoseSwitchGPIO_Init(void);

extern TaskHandle_t BR_ServoMotorHandlerTaskHandler;
#endif
